/*
 * sk_track_object.c
 *
 *  Created on: Feb 28, 2011
 *      Author: asish
 */
#define MAX_COUNT 10000

#include <stdio.h>
#include <cv.h>
#include "sk_track_object.h"
#include "sk_util.h"
#include "sk_polygon.h"

#define DIST(a,b) ((((a).x) - ((b).x))*(((a).x) - ((b).x)) + (((a).y) - ((b).y))*(((a).y) - ((b).y)))

static IplImage *g_grey = 0, *g_prev_grey = 0, *g_pyramid = 0, *g_prev_pyramid = 0, *g_swap_temp,  *g_eig, *g_temp;
static CvPoint2D32f* g_points[2] = {0,0}, *g_swap_points;
static CvPoint2D32f g_currentPoints[4];
static char* g_status = 0;
static int g_count = 0;
int g_flags = 0;
static int g_runs = 0;

/*
 * Initialize the set of points to be tracked
 */
void
SK_InitObjectTracking(IplImage *l_p_img, CvPoint2D32f *initialPoints, int noOfInitialPoints)
{
	IplImage *l_tempImg,*l_thresholdedImage;
	int win_size = 10;
    assert(l_p_img  != NULL);
    int i;

    g_grey = cvCreateImage( cvGetSize(l_p_img), 8, 1 );
    g_prev_grey = cvCreateImage( cvGetSize(l_p_img), 8, 1 );
    g_pyramid = cvCreateImage( cvGetSize(l_p_img), 8, 1 );
    g_prev_pyramid = cvCreateImage( cvGetSize(l_p_img), 8, 1 );
    g_points[0] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(g_points[0][0]));
    g_points[1] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(g_points[0][0]));
    g_eig = cvCreateImage( cvGetSize(g_grey), 32, 1 );
    g_temp = cvCreateImage( cvGetSize(g_grey), 32, 1 );

    for (i = 0; i<noOfInitialPoints; ++i)
    {
    	g_points[0][i] = (initialPoints[i]);
    }
    g_count = noOfInitialPoints;

	cvCvtColor( l_p_img, g_grey, CV_BGR2GRAY );
	//l_thresholdedImage = SK_DoThreshold( l_tempImg, 50,255,CV_THRESH_BINARY);

	cvFindCornerSubPix( g_grey, g_points[0], g_count,
			cvSize(win_size,win_size), cvSize(-1,-1),
			cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));

    CV_SWAP( g_prev_grey, g_grey, g_swap_temp );
    CV_SWAP( g_prev_pyramid, g_pyramid, g_swap_temp );


	printf("Subpixel Points = (%g,%g),(%g,%g),(%g,%g),(%g,%g)\n",g_points[0][0].x,g_points[0][0].y,
			g_points[0][1].x,g_points[0][1].y,
			g_points[0][2].x,g_points[0][2].y,
			g_points[0][3].x,g_points[0][3].y);
    for (i = 0; i<noOfInitialPoints; ++i)
    {
    	initialPoints[i] = g_points[0][i];
    }

    g_status = (char*)cvAlloc(MAX_COUNT);
    g_flags = 0;

    //cvReleaseImage(&l_tempImg);
    //cvReleaseImage(&l_thresholdedImage);
}

char
SK_TrackObject(IplImage *l_p_image, char **l_status, CvPoint2D32f **l_p_currentPoints)
{
	/* TODO:
	 * Since the points tracked by Lucas Kanade are sub pixel accurate, they might be off from the actual pixels by some amount.
	 * So to find the best three represented points we may need to traverse the points array to find out the closest match.
	 * But for now use the first three tracked points.
	 */
	CvSeq* currSeq;
	CvSeq* approxSeq;
	char l_count = 0;
    double quality = 0.01;
    double min_distance = 10;
    int win_size = 10;
    CvRect l_roiRect;
    IplImage *tempImg;
    CvMemStorage* l_storage;
    CvSeqReader reader;
	CvPoint2D32f *polyPts;
  	int minx;
	int miny;
	int maxx;
	int maxy;
	int nearestPoint;
	int minDist;
	CvPoint pointA,pointB;
	CvPoint temp;
    int i = 0,j = 0;

    assert(l_p_image != NULL);

    //l_smoothedImage = SK_SmoothImage(l_image,CV_GAUSSIAN, 3, 3, 0, 0);
    cvCvtColor( l_p_image, g_grey, CV_BGR2GRAY );
    //tempImg = SK_DoThreshold(g_grey,50,200,CV_THRESH_BINARY);
    g_runs = (g_runs + 1)%RUNS_THRESH;
    if (g_count != 0)
    {

		cvCalcOpticalFlowPyrLK( g_prev_grey, g_grey, g_prev_pyramid, g_pyramid,
			g_points[0], g_points[1], g_count, cvSize(win_size,win_size), 3, g_status, 0,
			cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03), g_flags );
		g_flags |= CV_LKFLOW_PYR_A_READY;
		//l_storage = cvCreateMemStorage(0);
		//currSeq = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvPoint), l_storage );
		if (g_runs == 0)
		{
			minx = g_points[1][0].x;
			miny = g_points[1][0].y;
			maxx = g_points[1][0].x;
			maxy = g_points[1][0].y;

			for ( i=0; i<g_count; ++i)
			{
				CvPoint p = cvPointFrom32f(g_points[1][i]);
				//cvCircle(l_p_image,p,4,CV_RGB(255,0,0),-1,CV_AA,0);
				if (p.x <= minx)
				{
					minx = p.x;
				}
				else if (p.x >= maxx)
				{
					maxx = p.x;
				}
				if (p.y <= miny)
				{
					miny = p.y;
				}
				else if (p.y >= maxy)
				{
					maxy = p.y;
				}
				//cvSeqPush( currSeq, &p);
			}
			pointA = cvPoint(minx - 10,miny - 10);
			pointB = cvPoint(maxx + 10,maxy + 10);
			//approxSeq = SK_ApproxPoly(currSeq);
			//cvStartReadSeq( approxSeq, &reader, 0 );
			//cvRectangle(l_p_image,pointA,pointB,CV_RGB(0,0,255),2,CV_AA,0);
			i = 0;
			if (g_count == SK_InitPolygon(l_p_image,&pointA,&pointB,&polyPts))
			{
				// Find the nearest point from the points returned by Lucas Kanade
				for (i = 0; i<4; ++i)
				{
					nearestPoint = 0;
					minDist = DIST((g_points[1][i]),polyPts[0]);
					for (j =0; j<4; ++j)
					{
						int temp;
						if ((temp = DIST(polyPts[j],g_points[1][i])) < minDist)
						{
							nearestPoint = j;
							minDist = temp;
						}
					}
					//printf("(%g,%g)\n",(g_points[1][i]).x,(g_points[1][i]).y);
					g_points[1][i] = (polyPts[nearestPoint]);
				}
				//printf("--->\n");
				cvFindCornerSubPix( g_grey, g_points[1], g_count,
						cvSize(win_size,win_size), cvSize(-1,-1),
						cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
				for (i = 0;i<4;++i)
				{
					g_currentPoints[i] = g_points[1][i];
					//printf(" (%g,%g)\n",(g_points[1][i]).x,(g_points[1][i]).y);

				}

				l_count = g_count;
			}
			else
			{
				printf("Number of points approx = %d\n",g_count);
				l_count = 0;
			}
		}
		else
		{
			for ( i=0; i<g_count; ++i)
			{
				cvCircle(l_p_image,cvPointFrom32f(g_points[1][i]),4,CV_RGB(255,0,0),-1,CV_AA,0);
				//cvSeqPush( currSeq, &p);
			}
			// TODO: This is dangerous because the while loop might never terminate
			i = 0;
			while (i<4 && l_count < 4)
			{
				while (i<4 && !g_status[i++]);
				if (g_status[i-1])
					g_currentPoints[l_count++] = (g_points[1][i-1]);
			}
		}
        //CV_READ_SEQ_ELEM( g_currentPoints[0], reader );
        //CV_READ_SEQ_ELEM( g_currentPoints[1], reader );
        //CV_READ_SEQ_ELEM( g_currentPoints[2], reader );

    }
    else
    {
    	printf("There are no points to track\n");
    }

    CV_SWAP( g_prev_grey, g_grey, g_swap_temp );
    CV_SWAP( g_prev_pyramid, g_pyramid, g_swap_temp );
    CV_SWAP( g_points[0], g_points[1], g_swap_points );

    *l_p_currentPoints = g_currentPoints;
    *l_status = g_status;
    //cvReleaseMemStorage(&l_storage);
    //cvClearSeq(currSeq);
    //cvClearSeq(approxSeq);
    return l_count;
}
